/*
 * MyAction.hh
 *
 * Pract
 * This example file provides the general template for an action file.
 *
 */

#ifndef MY_ACTION_HH
#define MY_ACTION_HH

#include <iostream>
#include <fstream>
#include <vector>
#include <list>

#include "Action.hh"
#include "Laser.hh"
#include "Sense.hh"
#include "Camera.hh"

#include "DoorDetector.hh"
#include "MoveBehavior.hh"
#include "StayMiddleBehavior.hh"
#include "RotateBehavior.hh"

#include "math.h"
// ma serve??     #include <boost/concept_check.hpp>
#include "HoughDoorDetector.hh"



class MyAction: public Action {
public:

    MyAction(sense::Sense*,camera::Camera* c,std::string = "");

    ~MyAction();
    //double& atan2(double arg1, double arg2);

    double lenghtOfScanRange(line::point_type);
    void setMyNumber(int num) {
        myNumber=num;    
    }
protected:


    void update();
    void act();

private:
    static const unsigned short DEFAULT_DUMMY_CYCLES = 10; //cicli di attesa inutili per tolleranza visione intermittente

    unsigned short myNumber ; //NUMERO DELLA PORTA DA CERCARE
    unsigned int dummyC; //variabili per contare cicli di wait
    unsigned short dummyCycles;
    int numVolte[10];
    bool alreadyRotating;
    short fase ; // 0 = centrale, 1 = finale, 2 = rotate , 3 = vai via
    int cicli;
    int numVisto;
    int angoloFinaleCono;
    bool fromRotate;
    double velocitaFinale;
    double distanzaFinale;
    double door_distance;
    double lin_vel_final;
    int cicliVaiVia;
    sense::Sense *sense;
    camera::Camera *camera;
    HoughDoorDetector door;

    MoveBehavior move;
    StayMiddleBehavior middle;
    RotateBehavior rotate;

    double distance(line::point_type p1,line::point_type p2);



};

#endif /* RACEACTION_HH_ */
